#include <buola/image.h>

#include <buola/cv/corners.h>

#include <xmmintrin.h>

namespace buola { namespace cvi {

CCornerList::CCornerList()
//    :   mHBlkSize(7)
//    ,   mCorSize(9)
    :   mHavePatches(false)
    ,   mHaveStats(false)
{

}

CCornerList::CCornerList(const std::vector<std::pair<CPoint_d,float>> &pIn)
//    :   mHBlkSize(7)
//    ,   mCorSize(9)
    :   mHavePatches(false)
    ,   mHaveStats(false)
{
    Init(pIn);
}

CCornerList::~CCornerList()
{
    Clear();
}

void CCornerList::Clear()
{
    for(int i=0;i<mCorners.size();++i)
        delete mCorners[i];
    mCorners.clear();
    mHavePatches=false;
    mHaveStats=false;
}

void CCornerList::Init(const std::vector<std::pair<CPoint_d,float>> &pIn)
{
    Clear();

    mBBox={0,0,0,0};

    for(int i=0;i<pIn.size();++i)
    {
        CCorner *lNewCorn=new CCorner;
        lNewCorn->mPixPos=pIn[i].first;
        mCorners.push_back(lNewCorn);

        if(i==0)
        {
            mBBox={lNewCorn->mPixPos,lNewCorn->mPixPos};
        }
        else
        {
            mBBox.l=std::min(mBBox.l,lNewCorn->mPixPos.x);
            mBBox.r=std::max(mBBox.r,lNewCorn->mPixPos.x);
            mBBox.t=std::min(mBBox.t,lNewCorn->mPixPos.y);
            mBBox.b=std::max(mBBox.b,lNewCorn->mPixPos.y);
        }
    }
}

void CCornerList::CreatePatches(const img::CView_gray8 &pView)
{
    for(int i=0;i<mCorners.size();i++)
    {
        CCorner *lC=mCorners[i];

        int lXP=(int)(lC->mPixPos.x-3);
        int lYP=(int)(lC->mPixPos.y-3);

        if(!lC->mPatch)
        {
            lC->mPatch=alloc_array<float>(8*8,16);
            double lSum1=0.0;
            double lSum2=0.0;
            for(int y=0;y<8;y++)
            {
                for(int x=0;x<8;x++)
                {
                    float lVal=(float)pView(lXP+x,lYP+y);
                    lC->mPatch[y*8+x]=lVal;
                    lSum1+=(double)lVal;
                    lSum2+=(double)lVal*(double)lVal;
                }
            }

            lSum1/=64.0;
            lSum2/=64.0;
            lSum2=sqrt(lSum2-lSum1*lSum1);
            for(int n=0;n<64;n++)
                lC->mPatch[n]=(float)(((double)lC->mPatch[n]-lSum1)/lSum2);
        }
    }

    mHavePatches=true;
}

float cross_correlate_64(float *pLH,float *pRH)
{
    __m128 lSum=_mm_setzero_ps();
    for(int i=0;i<16;i++)
    {
        __m128 lLH=_mm_load_ps(pLH+i*4);
        __m128 lRH=_mm_load_ps(pRH+i*4);
        lSum=_mm_add_ps(lSum,_mm_mul_ps(lLH,lRH));
    }
    float lResult[4];
    _mm_storeu_ps(lResult,lSum);
    return lResult[0]+lResult[1]+lResult[2]+lResult[3];
}

/*namespace cvi*/ } /*namespace buola*/ }
